#include "MPU6050.h"

#define MPU6050_ADDRESS 0xD0

void MPU6050_WriteReg(uint8_t RegAddress, uint8_t Data) {
  I2C_Start();
  I2C_SendByte(MPU6050_ADDRESS);
  I2C_ReceiveAck();
  I2C_SendByte(RegAddress);
  I2C_ReceiveAck();
  I2C_SendByte(Data);
  I2C_ReceiveAck();
  I2C_Stop();
}

uint8_t MPU6050_ReadReg(uint8_t RegAddress) {
  uint8_t Data;
  I2C_Start();
  I2C_SendByte(MPU6050_ADDRESS);
  I2C_ReceiveAck();
  I2C_SendByte(RegAddress);
  I2C_ReceiveAck();

  I2C_Start();
  I2C_SendByte(MPU6050_ADDRESS | 0x01);
  I2C_ReceiveAck();
  Data = I2C_ReceiveByte();
  I2C_SendAck(1);
  I2C_Stop();

  return Data;
}

void MPU6050_Init(void) {
  MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x01);
  MPU6050_WriteReg(MPU6050_PWR_MGMT_2, 0x00);
  MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x09);
  MPU6050_WriteReg(MPU6050_CONFIG, 0x06);
  MPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x18);
  MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x18);
}

uint8_t MPU6050_GetID(void) { return MPU6050_ReadReg(MPU6050_WHO_AM_I); }

double MPU6050_Temp(void) {
  uint8_t TEMP_L, TEMP_H;
  int16_t TEMP;
  TEMP_H = MPU6050_ReadReg(MPU6050_TEMP_OUT_H);
  TEMP_L = MPU6050_ReadReg(MPU6050_TEMP_OUT_L);

  TEMP = (TEMP_H << 8) | TEMP_L;

  return ((double)TEMP / 340.0) + 36.53;
}

void MPU6050_GetData(int16_t *AccX, int16_t *AccY, int16_t *AccZ,
                     int16_t *GyroX, int16_t *GyroY, int16_t *GyroZ) {
  uint8_t DataH, DataL;

  DataH = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_H);
  DataL = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_L);
  *AccX = (DataH << 8) | DataL;

  DataH = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_H);
  DataL = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_L);
  *AccY = (DataH << 8) | DataL;

  DataH = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_H);
  DataL = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_L);
  *AccZ = (DataH << 8) | DataL;

  DataH = MPU6050_ReadReg(MPU6050_GYRO_XOUT_H);
  DataL = MPU6050_ReadReg(MPU6050_GYRO_XOUT_L);
  *GyroX = (DataH << 8) | DataL;

  DataH = MPU6050_ReadReg(MPU6050_GYRO_YOUT_H);
  DataL = MPU6050_ReadReg(MPU6050_GYRO_YOUT_L);
  *GyroY = (DataH << 8) | DataL;

  DataH = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_H);
  DataL = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_L);
  *GyroZ = (DataH << 8) | DataL;
}
